#include <csignal>
#include <string>
#include <glog/logging.h>
#include <yaml-cpp/yaml.h>
#include "basic/LoopFunc.h"
#include "robot/robot.h"

std::shared_ptr<Robot> robot;
std::string curl_ipv4 = "\" http://192.168.1.102:8666/sys/receiveMapFile";
typedef void (* signal_handler)(int);
extern "C"
void signal_handler_func(int signal_num) {
    robot->stop();
    std::exit(EXIT_SUCCESS);
}

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    FLAGS_log_dir = "/home/yhs/log";
    google::ParseCommandLineFlags(&argc, &argv, true);
    
    std::string config_yaml = "/home/yhs/.wrks/hkprinter/HKPrinter/config/config.yaml";
    if (argc == 2) {
        config_yaml = argv[1];
    }

    LOG(INFO) << "[MIDWARE] Load yaml from " << config_yaml;
    auto yaml = YAML::LoadFile(config_yaml);
    uint16_t udp_server_port = 0;
    uint16_t udp_client_port = 0;
    std::string map_path = "/home/yhs/maps";
    std::string curl_ip = "192.168.0.102";
    try {
        udp_server_port = yaml["UDP_SERVER_PORT"].as<uint16_t>();
        udp_client_port = yaml["UDP_CLIENT_PORT"].as<uint16_t>();
        map_path = yaml["MAPS_PATH"].as<std::string>();
        curl_ip = yaml["CURL_IPV4"].as<std::string>();
    } catch (...) {
        LOG(ERROR) << "[MIDWARE] Failed to parse yaml";
        std::exit(EXIT_FAILURE);
    }

    curl_ipv4 = "\" http://" + curl_ip + ":8666/sys/receiveMapFile";

    robot = std::make_shared<Robot>(udp_server_port, udp_client_port);
    robot->set_map_path(map_path);
    robot->thread_UDPServer();
    LoopFunc loop_op("loop_op", 0.1, 4, std::bind(&Robot::thread_op, robot));
    LoopFunc loop_send("loop_send", 0.1, 4, std::bind(&Robot::thread_UDPSend, robot));

    loop_op.detach();
    loop_send.detach();

    signal_handler p_signal = signal_handler_func;
    signal(SIGINT, p_signal);
    while(1) {
        sleep(10);
    }

    return EXIT_SUCCESS;
}